#include <stdlib.h>
#include "CCamera.h"
#include "CGui.h"
#include "CRecognition.h"
#include "CJosmParser.h"
#include "COsmMap.h"

bool stop = false;
CCamera* camera;
CGui* gui;
CRawImage *image;
SDL_Event *event;
CRecognition *recognition;

void processKeys()
{
	SDL_PollEvent(event);
	Uint8 *keys;
	keys = SDL_GetKeyState(NULL);
	if (keys[SDLK_ESCAPE]) stop = true;
	if (keys[SDLK_a]) camera->setBrightness(camera->getBrightness()+1);
	if (keys[SDLK_z]) camera->setBrightness(camera->getBrightness()-1);
	if (keys[SDLK_s]) camera->setExposition(camera->getBrightness()+1);
	if (keys[SDLK_x]) camera->setExposition(camera->getBrightness()-1);

        if (keys[SDLK_l]) recognition->learnPixel(&image->data[3*((image->width/2)+image->width*(image->height-10))]);
        if (keys[SDLK_n]) recognition->increaseTolerance();
        if (keys[SDLK_m]) recognition->decreaseTolerance();
}

int main(int argc,char* argv[])
{
        int* pathWidth;
        int* pathCenter;

        double positionLat, positionLon;

        positionLat = 48.1474329;
        positionLon = 17.0723671;

	// If argument is empty, terminate program
	if (argc < 2) {
		fprintf(stderr,"usage: %s cameraDevice\ncameraDevice : e.g. /dev/video0\n",argv[0]);
		return 0;
	}

	const char* cameraDevice = argv[1];

	// Create instances of camera, gui and image
	camera = new CCamera();
	gui = new CGui();
	image = new CRawImage();
        recognition = new CRecognition();

	// Camera init
	camera->init(cameraDevice);
        pathWidth = (int*)calloc(image->height,sizeof(int));
        pathCenter = (int*)calloc(image->height,sizeof(int));

        // Create OSM map storage
        COsmMap *map = new COsmMap();
        // Create (J)OSM map parser
        CJosmParser *parser = new CJosmParser();
        // Vector for path
        std::vector<COsmNode > path;

        // Parse (J)OSM map
        stop = !parser->LoadMap("../map/locen.osm",map);
        if(!stop)
        {
            // Find path betwen two ways
            stop = !map->findPathID(418859608,777055184,path);
            map->predrawMap();
            parser->SaveMap("output.osm",map);
        }
        else
            std::cerr << "Fail to parse map" << endl;
        if(stop)
            std::cerr << "Path not exist" << endl;

        // Program loop
	while (!stop){
		// Load image
		camera->renewImage(image);
                // Process keyboard (and mouse) events
                processKeys();

                // Find path in image
                recognition->findPath(image,pathWidth,pathCenter);

                // Draw map into image
                map->drawMap(image,&positionLat,&positionLon);

		// Draw processed image
		gui->drawImage(image);
		// Draw GUI
		gui->update();	
	}
        delete recognition;
	delete camera;
	delete gui;
	return 0;
}
